#include <Wire.h>

#define I2C_ADDR        0x34

#define ADC_BAT_ADDR                  0x00
#define MOTOR_TYPE_ADDR               0x14 //编码电机类型设置
#define MOTOR_ENCODER_POLARITY_ADDR   0x15 //设置编码方向极性，
#define MOTOR_FIXED_PWM_ADDR          0x1F //固定PWM控制，属于开环控制,范围（-100~100）   
#define MOTOR_FIXED_SPEED_ADDR        0x33 //固定转速控制，属于闭环控制，
#define MOTOR_ENCODER_TOTAL_ADDR      0x3C //4个编码电机各自的总脉冲值

//电机类型具体值
#define MOTOR_TYPE_WITHOUT_ENCODER        0 //无编码器的电机,磁环每转是44个脉冲减速比:90  默认
#define MOTOR_TYPE_TT                     1 //TT编码电机
#define MOTOR_TYPE_N20                    2 //N20编码电机
#define MOTOR_TYPE_JGB37_520_12V_110RPM   3 //磁环每转是44个脉冲   减速比:90  默认

uint8_t data[20]; //用于暂存I2C读取的数据

// 初始化I2C引脚
#define SDA_PIN 21 // ESP32的默认SDA引脚
#define SCL_PIN 22 // ESP32的默认SCL引脚

bool WireWriteByte(uint8_t val) {
    Wire.beginTransmission(I2C_ADDR);
    Wire.write(val);
    if (Wire.endTransmission() != 0) {
        return false;
    }
    return true;
}

bool WireWriteDataArray(uint8_t reg, uint8_t *val, unsigned int len) {
    Wire.beginTransmission(I2C_ADDR);
    Wire.write(reg);
    for (unsigned int i = 0; i < len; i++) {
        Wire.write(val[i]);
    }
    if (Wire.endTransmission() != 0) {
        return false;
    }
    return true;
}

bool WireReadDataByte(uint8_t reg, uint8_t &val) {
    if (!WireWriteByte(reg)) {
        return false;
    }
    Wire.requestFrom(I2C_ADDR, 1);
    while (Wire.available()) {
        val = Wire.read();
    }
    return true;
}

int WireReadDataArray(uint8_t reg, uint8_t *val, unsigned int len) {
    if (!WireWriteByte(reg)) {
        return -1;
    }
    Wire.requestFrom(I2C_ADDR, len);
    unsigned int i = 0;
    while (Wire.available() && i < len) {
        val[i] = Wire.read();
        i++;
    }
    return i;
}

uint8_t MotorType = MOTOR_TYPE_JGB37_520_12V_110RPM; //设定电机类型
uint8_t MotorEncoderPolarity = 0; 

void setup() {
    Wire.begin(SDA_PIN, SCL_PIN); // 初始化I2C，指定SDA和SCL引脚
    Serial.begin(9600); // 初始化串口，波特率设置为9600
    delay(200);
    WireWriteDataArray(MOTOR_TYPE_ADDR, &MotorType, 1); // 在电机类型地址中写入电机类型编号
    delay(5);
    WireWriteDataArray(MOTOR_ENCODER_POLARITY_ADDR, &MotorEncoderPolarity, 1);
}

int8_t p1[4] = {50, 50, 50, 50};
int8_t p2[4] = {-20, -20, -20, -20};
int8_t s1[4] = {50, 50, 50, 50};
int8_t s2[4] = {-20, -20, -20, -20};
int8_t stop[4] = {0, 0, 0, 0};
int32_t EncodeTotal[4]; // 用于暂存电机累积转动量的值，正转递增，反转递减

void loop() {
    uint16_t v; // 用于暂存电压值
    WireReadDataArray(ADC_BAT_ADDR, data, 2); // 读取电压地址中存放的电压
    v = data[0] + (data[1] << 8);
    Serial.print("V = "); Serial.print(v); Serial.println("mV"); // 将电压转换为mV

    WireReadDataArray(MOTOR_ENCODER_TOTAL_ADDR, (uint8_t*)EncodeTotal, 16); // 读取电机累积转动量
    Serial.printf("Encode1 = %ld  Encode2 = %ld  Encode3 = %ld  Encode4 = %ld\n", EncodeTotal[0], EncodeTotal[1], EncodeTotal[2], EncodeTotal[3]);

    WireWriteDataArray(MOTOR_FIXED_SPEED_ADDR, (uint8_t*)p1, 4); // 设置电机速度
    delay(3000);

    WireWriteDataArray(MOTOR_FIXED_SPEED_ADDR, (uint8_t*)p2, 4); // 设置电机速度
    delay(3000);

    // PWM控制，注意：PWM控制是一个持续控制的过程，若有延时则会打断电机的运行（打印也会有一定的延时）
    // WireWriteDataArray(MOTOR_FIXED_PWM_ADDR, (uint8_t*)s1, 4);
}